#include "common.h"

// elmult: Takes two complex or two real matrices A, B and returns (in MATLAB notation)  A.*B, 
Matcp elmult(Matcp A, Matcp B)
{
	const int Arows = A.rows;
	const int Acols = A.cols;
	Matcp C = Matcp(Arows, Acols, (0.0,0.0));
	
	// pointers to elements of A,B,C respectively
	cptype * Ael = A.ptr<cptype>(0);
	cptype * Bel = B.ptr<cptype>(0);
	cptype * Cel = C.ptr<cptype>(0);

	for(int i = 0; i < Arows * Acols; i++)
	{
		if(i > 0)
		{   // increment to the next array element
			Ael++;
			Bel++;
			Cel++;
		}
		// multiply, yo
		(*Cel) = (*Ael) * (*Bel);
	}
	return C;
}

Matrl elmult(Matrl A, Matrl B)
{
	const int Arows = A.rows;
	const int Acols = A.cols;
	Matrl C = Matrl(Arows, Acols, (0.0,0.0));
	
	// pointers to elements of A,B,C respectively
	rltype * Ael = A.ptr<rltype>(0);
	rltype * Bel = B.ptr<rltype>(0);
	rltype * Cel = C.ptr<rltype>(0);

	for(int i = 0; i < Arows * Acols; i++)
	{
		if(i > 0)
		{   // increment to the next array element
			Ael++;
			Bel++;
			Cel++;
		}
		// multiply, yo
		(*Cel) = (*Ael) * (*Bel);
	}
	return C;
}

// eldiv: Takes two complex or two real matrices A, B and returns (in MATLAB notation)  A./B, 
Matcp eldiv(Matcp A, Matcp B)
{
	const int Arows = A.rows;
	const int Acols = A.cols;
	Matcp C = Matcp(Arows, Acols, (0.0,0.0));
	
	// pointers to elements of A,B,C respectively
	cptype * Ael = A.ptr<cptype>(0);
	cptype * Bel = B.ptr<cptype>(0);
	cptype * Cel = C.ptr<cptype>(0);

	for(int i = 0; i < Arows * Acols; i++)
	{
		if(i > 0)
		{   // increment to the next array element
			Ael++;
			Bel++;
			Cel++;
		}
		// divide, yo
		(*Cel) = (*Ael) / (*Bel);
	}
	return C;
}

Matrl eldiv(Matrl A, Matrl B)
{
	const int Arows = A.rows;
	const int Acols = A.cols;
	Matrl C = Matrl(Arows, Acols, (0.0,0.0));
	
	// pointers to elements of A,B,C respectively
	rltype * Ael = A.ptr<rltype>(0);
	rltype * Bel = B.ptr<rltype>(0);
	rltype * Cel = C.ptr<rltype>(0);

	for(int i = 0; i < Arows * Acols; i++)
	{
		if(i > 0)
		{   // increment to the next array element
			Ael++;
			Bel++;
			Cel++;
		}
		// divide, yo
		(*Cel) = (*Ael) / (*Bel);
	}
	return C;
}

// merge2:  merge two real matrices into one complex matrix
Matcp merge2(Matrl realplane, Matrl cplxplane)
{
	Matcp outputmat;
	Matrl planes[] = {realplane, cplxplane};
	merge(planes, 2, outputmat);
	return outputmat;
}


// cvabs: Given complex matrix M, return real matrix Mabs = |M|
Matrl cvabs(Matcp M)
{
	const int Mrows = M.rows;
	const int Mcols = M.cols;
	Matrl Mabs = Mat(Mrows, Mcols, CV_RL);

	// pointers to the first elements in Mel and Mabsel
	cptype * Mel = M.ptr<cptype>(0);
	rltype * Mabsel = Mabs.ptr<rltype>(0);

	for(int i = 0; i < Mrows * Mcols; i++)
	{
		if(i > 0)
		{   // increment to next array element
			Mabsel++;
			Mel++;
		}
		*Mabsel = abs(*Mel);
	}

	return Mabs;
}
		
// read_image: read in a grayscale image and convert it to a normalized complex matrix.
Matcp read_image(string filein)
{
	Mat imin = imread(filein, 0); // the zero means grayscale
	Matcp imout; // this will contain the function output
	imin.convertTo(imin, CV_RL);
	imin *= 1.0/255.0;
	imout = merge2(imin, Mat::zeros(imin.size(), CV_RL));
	return imout;
}

// addscalar: Takes a complex matrix A and a real scalar c and returns A + c (MATLAB notation)
Matcp addscalar(Matcp A, rltype c)
{
	Matcp B = A.clone();
	// Since B is complex, the real pointer Bel will increment as
	// real(B(0)), imag(B(0)), real(B(1)), imag(B(1)), etc.
	rltype * Bel = B.ptr<rltype>(0);
	for(int i = 0; i < A.rows * A.cols; i++)
	{
		if(i>0){Bel++; Bel++;} // Increment to next real part
		*Bel = *Bel + c;
	}
	return B;
}

Matrl thresholdtz64(Matrl src, rltype thresh)
// Implements opencv's threshold, the THRESH_TOZERO option, compatible with 64 bit depth (32 also ok).
{
	const int srcrows = src.rows;
	const int srccols = src.cols;
	Matrl dst(srcrows, srccols, 0.0);

	rltype * dstel = dst.ptr<rltype>(0);
	rltype * srcel = src.ptr<rltype>(0);

	for(int i = 0; i < srcrows * srccols; i++)
	{
		if(i > 0){dstel++; srcel++;}

		if((*srcel) > thresh)
		{ *dstel = *srcel; }
		else { *dstel = 0.0; }
	}
	return dst;
}

Matrl thresholdbi64(Matrl src, rltype thresh, rltype maxval)
// Implements opencv's threshold, the THRESH_BINARY_INV option, compatible with 64 bit depth (32 also ok).
{
	const int srcrows = src.rows;
	const int srccols = src.cols;

	Matrl dst(srcrows, srccols, 0.0);

	rltype * dstel = dst.ptr<rltype>(0);
	rltype * srcel = src.ptr<rltype>(0);

	for(int i = 0; i < srcrows * srccols; i++)
	{
		if(i > 0){dstel++; srcel++;}

		if((*srcel) > thresh)
		{ *dstel = 0.0; }
		else {*dstel = maxval;}
	}
	return dst;
}